/*
  Seed-Planting Rover - top-level controller.
*/

#include <Wire.h>
#include <Adafruit_MotorShield.h>

#include "Config.h"
#include "StorageManager.h"
#include "SensorSuite.h"
#include "PowerManager.h"
#include "Drivetrain.h"
#include "Dispenser.h"
#include "DisplayManager.h"
#include "SerialCommands.h"

// Subsystem instances
Adafruit_MotorShield AFMS;
StorageManager storage;
SensorSuite sensors;
PowerManager power;
Drivetrain drivetrain;
Dispenser dispenser;
DisplayManager display;
SerialCommands commands;

// Top-level state machine
enum class RobotState : uint8_t { INIT, SENSING, AVOIDING, DRIVING, PLANTING, OUT_OF_SEEDS };
RobotState state = RobotState::INIT;

bool actionStarted = false;        
bool pendingPlantAfterHop = false; 
unsigned long lastPollMs = 0;
unsigned long lastTelemetryMs = 0;

const __FlashStringHelper* stateName(RobotState s) {
  switch (s) {
    case RobotState::INIT:         return F("INIT");
    case RobotState::SENSING:      return F("SENSING");
    case RobotState::AVOIDING:     return F("AVOIDING");
    case RobotState::DRIVING:      return F("DRIVING");
    case RobotState::PLANTING:     return F("PLANTING");
    case RobotState::OUT_OF_SEEDS: return F("OUT_OF_SEEDS");
  }
  return F("UNKNOWN");
}

void setup() {
  Serial.begin(115200);
  Wire.begin();

  storage.begin();
  display.begin();
  sensors.begin(&storage);
  randomSeed((unsigned long)micros());

  if (!AFMS.begin()) {
    Serial.println(F("FATAL: motor shield not detected on I2C bus"));
    pinMode(Pins::LED_RED, OUTPUT);
    while (true) { // halt safely rather than run with no drive control
      digitalWrite(Pins::LED_RED, HIGH); delay(150);
      digitalWrite(Pins::LED_RED, LOW);  delay(150);
    }
  }
  drivetrain.begin(AFMS);
  dispenser.begin(Pins::ACTUATOR_SERVO);
  commands.begin();
  pinMode(Pins::HEARTBEAT, OUTPUT);

  Serial.println(F("time_ms,state,mode,humidity,light_pct,soil_pct,battery_pct,"
                    "dist_cm,suitable,heading,session_seeds,lifetime_seeds,hopper_empty"));
}

void loop() {
  unsigned long now = millis();

  storage.tickRuntime(now);
  storage.flushIfDue(now);
  commands.poll(sensors, storage, dispenser, power);

  // always advance, regardless of top-level state.
  drivetrain.update(now);
  dispenser.update(now, sensors.readHopperLive());

  // A plant cycle completing 
  bool seedDetected;
  if (dispenser.consumeCompletedCycle(seedDetected)) {
    actionStarted = false;
    if (seedDetected) {
      storage.addSeeds(1);
    }
    if (dispenser.isHopperEmpty()) {
      display.showOutOfSeeds(storage.lifetimeSeedCount());
      state = RobotState::OUT_OF_SEEDS;
    } else {
      state = RobotState::SENSING;
    }
  }

  switch (state) {
    case RobotState::INIT:
      state = RobotState::SENSING;
      break;

    case RobotState::SENSING: {
      unsigned long pollInterval = power.pollIntervalMs();
      if (now - lastPollMs >= pollInterval) {
        sensors.poll(now);
        power.update(sensors.batteryPct(), now);
        lastPollMs = now;

        display.update(now, dispenser.sessionSeedCount(), sensors.isConditionSuitable(),
                       power.backlightShouldBeOn(now));

        if (sensors.isObstacleDetected()) {
          state = RobotState::AVOIDING;
        } else if (dispenser.isHopperEmpty()) {
          display.showOutOfSeeds(storage.lifetimeSeedCount());
          state = RobotState::OUT_OF_SEEDS;
        } else {
          state = RobotState::DRIVING;
        }
      }
      break;
    }

    case RobotState::AVOIDING:
      if (!drivetrain.isBusy() && !actionStarted) {
        drivetrain.startAvoidanceManeuver(power.motorSpeedScale());
        display.showTransientMessage("Changing Direction", 2000);
        actionStarted = true;
      } else if (actionStarted && !drivetrain.isBusy()) {
        actionStarted = false;
        state = RobotState::SENSING;
      }
      break;

    case RobotState::DRIVING:
      if (!drivetrain.isBusy() && !actionStarted) {
        bool skipForConservation = power.shouldSkipThisPlantingOpportunity();
        pendingPlantAfterHop = sensors.isConditionSuitable() && !skipForConservation;
        drivetrain.startForwardHop(power.hopDurationMs(), power.motorSpeedScale());
        actionStarted = true;
      } else if (actionStarted && !drivetrain.isBusy()) {
        actionStarted = false;
        state = pendingPlantAfterHop ? RobotState::PLANTING : RobotState::SENSING;
      }
      break;

    case RobotState::PLANTING:
      if (!dispenser.isBusy() && !actionStarted) {
        dispenser.startPlantCycle();
        actionStarted = true;
      }
      // Completion (success or hopper-empty) 
      break;

    case RobotState::OUT_OF_SEEDS:
      drivetrain.emergencyStop();
      display.updateOutOfSeedsBlink(now);
      if (!dispenser.isHopperEmpty()) { // cleared by a REFILL_HOPPER serial command
        display.resetAfterRefill();
        state = RobotState::SENSING;
      }
      break;
  }

  if (now - lastTelemetryMs >= 1000) {
    Serial.print(now);                             Serial.print(',');
    Serial.print(stateName(state));                Serial.print(',');
    Serial.print(power.modeName());                Serial.print(',');
    Serial.print(sensors.humidityPct(), 1);         Serial.print(',');
    Serial.print(sensors.lightPct(), 1);            Serial.print(',');
    Serial.print(sensors.soilPct(), 1);             Serial.print(',');
    Serial.print(sensors.batteryPct(), 1);          Serial.print(',');
    Serial.print(sensors.distanceCm(), 1);          Serial.print(',');
    Serial.print(sensors.isConditionSuitable());    Serial.print(',');
    Serial.print(drivetrain.headingSign());         Serial.print(',');
    Serial.print(dispenser.sessionSeedCount());     Serial.print(',');
    Serial.print(storage.lifetimeSeedCount());      Serial.print(',');
    Serial.println(dispenser.isHopperEmpty());
    digitalWrite(Pins::HEARTBEAT, !digitalRead(Pins::HEARTBEAT));
    lastTelemetryMs = now;
  }
}
